package ar.edu.untref.robotica.simulador;

import java.io.IOException;
import java.math.BigDecimal;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.CopyOnWriteArrayList;

import ar.edu.untref.robotica.control.ModuloDeControl;
import ar.edu.untref.robotica.control.Teclado;
import ar.edu.untref.robotica.log.Log;
import ar.edu.untref.robotica.motorfisico.Calculadora;

public class Simulador extends TimerTask {

	private int fps = 120;
	private Graficador graficador;

	private CopyOnWriteArrayList<ParEscenarioRobot> parEscenarioRobot;


	private Escenario escenario;
	private Robot robot;

	private double velocidadFinalDeseadaIzquierda = 0;
	private double velocidadFinalDeseadaDerecha = 0;

	private double fuerzaAceleracion = 0.05;
	private Posicion posicionAnterior;
	private ModuloDeControl controlador;
	private Log logger;
	private int contadorLog = 0;

	public Simulador(CopyOnWriteArrayList<ParEscenarioRobot> parEscenarioRobot) {
		this.parEscenarioRobot = parEscenarioRobot;
	}

	public void inicializar() {

		this.graficador = new Graficador(this.parEscenarioRobot, this);
		for (ParEscenarioRobot cadaParEscenarioRobot : this.parEscenarioRobot) {
			cadaParEscenarioRobot.setRobot(new Robot(Configuracion.instancia().getAltoRobot(),
					Configuracion.instancia().getAnchoRobot()));
		}
		Timer timer = new Timer();
		double period = (double) 1 / this.fps * 1000;
		timer.scheduleAtFixedRate(this, 0, (long) period);

		this.logger = new Log();
		try {
			this.logger.crearFile();
		} catch (IOException e) {

			e.printStackTrace();
		}

		this.controlador = new Teclado(this, this.parEscenarioRobot.get(0));
	}

	@Override
	public void run() {

		this.contadorLog++;
		Iterator<ParEscenarioRobot> iterador = this.parEscenarioRobot.iterator();
		while(iterador.hasNext()) {
			ParEscenarioRobot cadaParEscenarioRobot = iterador.next();
//		for (ParEscenarioRobot cadaParEscenarioRobot : this.parEscenarioRobot) {
			//ParEscenarioRobot cadaParEscenarioRobot = this.parEscenarioRobot[0];
			if (Calculadora.getVelocidadDesplazamiento(
					cadaParEscenarioRobot.getVelocidadFinalDeseadaIzquierda(),
					cadaParEscenarioRobot.getVelocidadFinalDeseadaDerecha()) > 0) {
				if (!cadaParEscenarioRobot.puedeAvanzar()) {
					acelerarRobot(0, 0, cadaParEscenarioRobot);
				}
			} else if (Calculadora.getVelocidadDesplazamiento(
					cadaParEscenarioRobot.getVelocidadFinalDeseadaIzquierda(),
					cadaParEscenarioRobot.getVelocidadFinalDeseadaDerecha()) < 0) {
				if (!cadaParEscenarioRobot.puedeRetroceder()) {
					acelerarRobot(0, 0, cadaParEscenarioRobot);
				}
			}

			this.logicaAceleracion();
			cadaParEscenarioRobot.getEscenario().aumentarAngulo(Calculadora.getCambioAngulo(
					cadaParEscenarioRobot.getRobot().getRuedaI(), cadaParEscenarioRobot.getRobot().getRuedaD())
					/ this.fps);
			this.reflejarMovimiento();
			this.graficador.sensar(cadaParEscenarioRobot.getRobot(), cadaParEscenarioRobot.getEscenario());

			//			if (this.contadorLog == 60){
			//				
			//				try {
			//					
			//					this.logger.guardarDatos(graficador.datosRelevantesAGuardar());
			//					
			//				} catch (IOException e) {
			//				
			//					e.printStackTrace();
			//				}
			//				
			//				this.contadorLog = 0;
			//			}

			//this.menuComandosRobot();
			if (cadaParEscenarioRobot.hayChoque() || hayChoqueEntreRobots(cadaParEscenarioRobot.getNumeroRobot())) {
				cadaParEscenarioRobot.getRobot().getRuedaD().setVelocidad(new BigDecimal(0));
				cadaParEscenarioRobot.getRobot().getRuedaI().setVelocidad(new BigDecimal(0));
				this.acelerarRobot(0, 0, cadaParEscenarioRobot);
				cadaParEscenarioRobot.getEscenario().setPosicionRobot(cadaParEscenarioRobot.getPosicionAnterior());
			} else if (!cadaParEscenarioRobot.hayChoque() && !hayChoqueEntreRobots(cadaParEscenarioRobot.getNumeroRobot())){
				cadaParEscenarioRobot.setPosicionAnterior(cadaParEscenarioRobot.getEscenario().getPosicionRobot().clone());
			}

		}


	}

	public void reflejarMovimiento() {
		for (ParEscenarioRobot cadaParEscenarioRobot : this.parEscenarioRobot) {
			Posicion posicionRobot = cadaParEscenarioRobot.getEscenario().getPosicionRobot();
			double posicionXActual = posicionRobot.getPosicionX();
			double posicionYActual = posicionRobot.getPosicionY();
			double velocidad = cadaParEscenarioRobot.getRobot().getVelocidadDesplazamiento();
			double angulo = Math.toRadians(cadaParEscenarioRobot.getEscenario().getPosicionRobot()
					.getAngulo());
			double posicionXNueva = posicionXActual + velocidad * Math.sin(angulo);
			double posicionYNueva = posicionYActual - velocidad * Math.cos(angulo);
			posicionRobot.setPosicionX(posicionXNueva);
			posicionRobot.setPosicionY(posicionYNueva);
			this.graficador.repaint();

		}

	}

	private void logicaAceleracion() {
		for (ParEscenarioRobot cadaParEscenarioRobot : this.parEscenarioRobot) {
			BigDecimal deltaDeAceleracion = new BigDecimal(this.fuerzaAceleracion);
			if (!cadaParEscenarioRobot.velocidadDeseadaAlcanzadaIzquierda()) {
				if (cadaParEscenarioRobot.getVelocidadFinalDeseadaIzquierda() > cadaParEscenarioRobot.getRobot().getRuedaI()
						.getVelocidad().doubleValue()) {
					cadaParEscenarioRobot.getRobot().getRuedaI().setVelocidad(
							cadaParEscenarioRobot.getRobot().getRuedaI().getVelocidad()
							.add(deltaDeAceleracion));
				} else if (cadaParEscenarioRobot.getVelocidadFinalDeseadaIzquierda() < cadaParEscenarioRobot.getRobot()
						.getRuedaI().getVelocidad().doubleValue()) {
					cadaParEscenarioRobot.getRobot().getRuedaI().setVelocidad(
							cadaParEscenarioRobot.getRobot().getRuedaI().getVelocidad()
							.subtract(deltaDeAceleracion));
				}
			}
			if (!cadaParEscenarioRobot.velocidadDeseadaAlcanzadaDerecha()) {
				if (cadaParEscenarioRobot.getVelocidadFinalDeseadaDerecha() > cadaParEscenarioRobot.getRobot().getRuedaD()
						.getVelocidad().doubleValue()) {
					cadaParEscenarioRobot.getRobot().getRuedaD().setVelocidad(
							cadaParEscenarioRobot.getRobot().getRuedaD().getVelocidad()
							.add(deltaDeAceleracion));
				} else if (cadaParEscenarioRobot.getVelocidadFinalDeseadaDerecha() < cadaParEscenarioRobot.getRobot()
						.getRuedaD().getVelocidad().doubleValue()) {
					cadaParEscenarioRobot.getRobot().getRuedaD().setVelocidad(
							cadaParEscenarioRobot.getRobot().getRuedaD().getVelocidad()
							.subtract(deltaDeAceleracion));
				}
			}
		}
	}

	public void acelerarRobot(double velocidadFinalIzquierda,
			double velocidadFinalDerecha, ParEscenarioRobot parEscenarioRobot) {
		if (parEscenarioRobot == null) {
//			for (ParEscenarioRobot cadaParEscenarioRobot : this.parEscenarioRobot) {
//				cadaParEscenarioRobot.setVelocidadFinalDeseadaIzquierda(velocidadFinalIzquierda);
//				cadaParEscenarioRobot.setVelocidadFinalDeseadaDerecha(velocidadFinalDerecha);
//			}
		}else{
			System.out.println("ACA " + parEscenarioRobot.getNumeroRobot() );
			parEscenarioRobot.setVelocidadFinalDeseadaIzquierda(velocidadFinalIzquierda);
			parEscenarioRobot.setVelocidadFinalDeseadaDerecha(velocidadFinalDerecha);
		}
	}

	private void menuComandosRobot() {

		/*final Integer valorRuedaDerecha = (Integer) this.graficador
				.getSpinnerD().getValue();
		final Integer valorRuedaIzquierda = (Integer) this.graficador
				.getSpinnerI().getValue();
		this.graficador.getSpinnerD().addChangeListener(new ChangeListener() {

			@Override
			public void stateChanged(ChangeEvent e) {

				Integer valorAntRD = (Integer) ((JSpinner) e.getSource())
						.getPreviousValue();
				if (valorRuedaDerecha > valorAntRD) {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda,
							Simulador.this.velocidadFinalDeseadaDerecha - 1);
				} else {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda,
							Simulador.this.velocidadFinalDeseadaDerecha + 1);
				}
				Simulador.this.graficador.getBarraDer().setValue(
						(int) Simulador.this.graficador.getSpinnerD()
						.getValue());
			}
		});
		this.graficador.getSpinnerI().addChangeListener(new ChangeListener() {

			@Override
			public void stateChanged(ChangeEvent e) {

				Integer valorAntRI = (Integer) ((JSpinner) e.getSource())
						.getPreviousValue();
				if (valorRuedaIzquierda > valorAntRI) {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda - 1,
							Simulador.this.velocidadFinalDeseadaDerecha);
				} else {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda + 1,
							Simulador.this.velocidadFinalDeseadaDerecha);
				}
				Simulador.this.graficador.getBarraIzq().setValue(
						(int) Simulador.this.graficador.getSpinnerI()
						.getValue());
			}
		});*/
	}

	private boolean hayChoqueEntreRobots(int numeroRobot) {

		boolean choco = false;
		for (int i = 0 ; i < this.parEscenarioRobot.size() ; i++) {
				if (numeroRobot != this.parEscenarioRobot.get(i).getNumeroRobot() && this.parEscenarioRobot.get(numeroRobot-1).getEscenario().getShapeRobot().intersects(this.parEscenarioRobot.get(i).getEscenario().getShapeRobot().getBounds2D())) {
					choco = true;
				}
		}
			
		return choco;
	}

	public double getVelocidadFinalDeseadaIzquierda(){

		return velocidadFinalDeseadaIzquierda;
	}

	public double getVelocidadFinalDeseadaDerecha(){

		return velocidadFinalDeseadaDerecha;
	}

	public Escenario getEscenario() {
		return escenario;
	}

	public Robot getRobot() {
		return robot;
	}

	public Graficador getGraficador(){
		return graficador;
	}
	
	public void setRobotAControl(int numeroDeRobot) {
		this.controlador.setParEscenarioRobot(null);
		this.controlador = null;
		this.controlador = new Teclado(this, this.parEscenarioRobot.get(numeroDeRobot));
//		this.controlador.setParEscenarioRobot(this.parEscenarioRobot.get(numeroDeRobot));
		this.graficador.setFocusable(true);
	}

}
